#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>

#include "ap_assert.h"
#include "types.h"
#include "net_hdr.h"
#include "gtp.h"
#include "pdp.h"
#include "error.h"
#include "timer.h"
#include "user_packet.h"
#include "pkt_pcap.h"



u16 gtpc_port = GTPv1C_PORT;
u16 gtpu_port = GTPv1U_PORT;

#if defined(CONFIG_NETWORK_WCDMA)

int gtpc_cap_enable = 0;
struct raw_pcap *gtpc_pcap = NULL;


static inline void imsi_to_str(const u8 *ad, char *str)
{
    u8 bits8to5, bits4to1;
    int i, j = 0;
    for (i = 0; i < 8; i++) {
        bits8to5 = (ad[i] >> 4) & 0x0F;
        bits4to1 = ad[i] & 0x0F;
 
        if (bits4to1 < 0xA)
            str[j++] = bits4to1 + 0x30;
        if (bits8to5 < 0xA)
            str[j++] = bits8to5 + 0x30;
    }
    str[j] = '\0';
}

inline u64 imsi_to_ull(const u8 *ad)
{
    char str[18];
    imsi_to_str(ad, str);
    return strtoll(str, NULL, 10);
}

static inline void msisdn_to_str(const u8 *ad, u16 len, char *str)
{
    u8 bits8to5, bits4to1;
    int i, j = 0;
    for (i = 1; i < len && i < 9; i++) {
        bits8to5 = (ad[i] >> 4) & 0x0F;
        bits4to1 = ad[i] & 0x0F;
        if (bits4to1 < 0xA)
            str[j++] = bits4to1 + 0x30;
        if (bits8to5 < 0xA)
            str[j++] = bits8to5 + 0x30;
    }
    str[j] = '\0';
}

inline u64 msisdn_to_ull(const u8 *ad, u16 len)
{
    char str[18];
    msisdn_to_str(ad, len, str);
    return strtoll(str, NULL, 10);
}

static inline void digital_to_str(const u8 *ad, u16 len, char *str)
{
    u8 bits8to5, bits4to1;
    int i, j = 0;

    for (i = 0; i < len; i++) {
        bits8to5 = (ad[i] >> 4) & 0x0F;
        bits4to1 = ad[i] & 0x0F;
 
        if (bits4to1 < 0xA)
            str[j++] = bits4to1 + 0x30;
        if (bits8to5 < 0xA)
            str[j++] = bits8to5 + 0x30;
    }
    str[j] = '\0';
}

u64 imeisv_to_ull(const u8 *ad, u16 len)
{
    char str[20];   /* imei(sv) length */
    if (len > 9) 
        return 0;
    digital_to_str(ad, len, str);
    return strtoll(str, NULL, 10);
}

/* Decoders of fields in extension headers, each function returns no of bytes from field */

/* GPRS:    9.60 v7.6.0, chapter
 * UMTS:    29.060 v4.0, chapter
 * Type = 1 (Decimal)
 */
static u32 decode_gtp_cause(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1];
    return 2;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.2
 * UMTS:    29.060 v4.0, chapter 7.7.2
 * Type = 2(Decimal)
 */
static inline u32 decode_gtp_imsi(u8 *tv, void *out)
{
   // *((u64 *)(out)) = imsi_to_ull(tv + 1);
    struct imsi *imsi = (struct imsi *)out;
    imsi_to_str((tv + 1), imsi->str);
    imsi->dig = strtoll(imsi->str, NULL, 10);

    return 9;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.3
 * UMTS:    29.060 v4.0, chapter 7.7.3
 * Type = 3(Decimal)
 */
static u32 decode_gtp_rai(u8 *tv, void *out)
{
    u8  byte[3];
    u16 mnc, mcc;
    struct RAI *rai = (struct RAI *)out;

    byte[0] = tv[1];
    byte[1] = tv[2];
    byte[2] = tv[3];
    
    mcc = (byte[0] & 0x0F) * 100 + ((byte[0] & 0xF0) >> 4) * 10 + (byte[1] & 0x0F);

    if ((byte[1] & 0xF0) == 0xF0)
        mnc = (byte[2] & 0x0F) * 10 + ((byte[2] & 0xF0) >> 4);
    else
        mnc = (byte[2] & 0x0F) * 100 + ((byte[2] & 0xF0) >> 4) * 10 + ((byte[1] & 0xF0) >> 4);

    rai->mcc = mcc;
    rai->mnc = mnc;
    rai->lac = ntohs(*(u16 *)(tv + 4));
    rai->rac = tv[6];

    return 7;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.4, page 39
 * UMTS:    29.060 v4.0, chapter 7.7.4, page 47
 * Type = 4 (Decimal)
 */
static u32 decode_gtp_tlli(u8 *tv, void *out)
{
    *(u32 *)out = ntohl(*(u32 *)(tv + 1));
    return 5;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.5, page 39
 * UMTS:    29.060 v4.0, chapter 7.7.5, page 47
 * Type = 5 (Decimal)
 */
static u32 decode_gtp_ptmsi(u8 *tv, void *out)
{
    *(u32 *)out = ntohl(*(u32 *)(tv + 1));
    return 5;
}


/* adjust - how many bytes before offset should be highlighted
 */
static u32 decode_qos_gprs(u8 *buf, void *out)
{
    struct QOS *qos = (struct QOS *)out;

    qos->spare1 = buf[0] & 0xC0 >> 6;
    qos->delay = buf[0] & 0x38 >> 3;
    qos->reliability = buf[0] & 0x07;
    qos->peak = buf[1] & 0xF0 >> 4;
    qos->spare2 = buf[1] & 0x08>>3;
    qos->precedence = buf[1] & 0x07;
    qos->spare3 = buf[2] & 0xE0>>5;
    qos->mean = buf[2] & 0x1F;

    return 3;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.6, page 39
 *         4.08
 *         3.60
 * UMTS:     not present
 * TODO:    check if length is included: ETSI 4.08 vs 9.60
 */
static u32 decode_gtp_qos_gprs(u8 *tv, void *out)
{
    return (1 + decode_qos_gprs(tv + 1, out));
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.7, page 39
 * UMTS:    29.060 v4.0, chapter 7.7.6, page 47
 * Type = 7 (Decimal)
 */
static u32 decode_gtp_reorder(u8 *tv, void *out)
{
    *(u8 *)out = tv[1] & 0x01;
    return 2;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.8, page 40
 *         4.08 v7.1.2, chapter 10.5.3.1+
 * UMTS:    29.060 v4.0, chapter 7.7.7
 * Type = 9 (Decimal)
 */
static u32 decode_gtp_auth_tri(u8 *tv, void *out)
{
    return 1 + 16 + 4 + 8;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.9, page 40
 *         9.02 v7.7.0, page 1090
 * UMTS:    29.060 v4.0, chapter 7.7.8, page 48
 *         29.002 v4.2.1, chapter 17.5, page 268
 * Type = 11 (Decimal)
 */
static u32 decode_gtp_map_cause(u8 *tv, void *out)
{
    *(u8 *)out = tv[1];
    return 2;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.10, page 41
 * UMTS:    29.060 v4.0, chapter 7.7.9, page 48
 * Type = 12 (Decimal)
 */
static u32 decode_gtp_ptmsi_sig(u8 *tv, void *out)
{
    *(u32 *)out = tv[1] << 16 | tv[2] << 8 | tv[3];
    return 4;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.11, page 41
 * UMTS:    29.060 v4.0, chapter 7.7.10, page 49
 * Type = 13 (Decimal)
 */
static u32 decode_gtp_ms_valid(u8 *tv, void *out)
{
    *(u8 *)out = tv[1] & 0x01;
    return 2;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.12, page 41
 * UMTS:    29.060 v4.0, chapter 7.7.11, page 49
 * Type = 14 (Decimal)
 */
static u32 decode_gtp_recovery(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1];
    return 2;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.13, page 41
 * UMTS:    29.060 v4.0, chapter 7.7.12, page 49
 * Type = 15 (Decimal)
 */
static u32 decode_gtp_sel_mode(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1];
    return 2;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.14, page 42
 * UMTS:    29.060 v4.0, chapter 7.7.13, page 50
 * Type = 16 (Decimal)
 */
static u32 decode_gtp_teid_data1(u8 *tv, void *out)
{
    *((u32 *)out) = ntohl(*(u32 *)(tv + 1));
    return 5;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.15, page 42
 * UMTS:    29.060 v4.0, chapter 7.7.14, page 42
 * Type = 17 (Decimal)
 */
static u32 decode_gtp_teid_cp(u8 *tv, void *out)
{
    *((u32 *)out) = ntohl(*(u32 *)(tv + 1));
    return 5;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.16, page 42
 * UMTS:    29.060 v4.0, chapter 7.7.15, page 51
 * Type = 18 (Decimal)
 */
static u32 decode_gtp_teid_data2(u8 *tv, void *out)
{
    struct TEID_DATA2 *data2 = (struct TEID_DATA2*)out;

    data2->nsapi = tv[1] & 0x0F;
    data2->teid_data2 = ntohl(*(u32 *)(tv + 2));
    return 6;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.16A, page 43
 * UMTS:    29.060 v4.0, chapter 7.7.16, page 51
 * Check if all ms_reason types are included
 * Type = 19 (Decimal)
 */
static u32 decode_gtp_tear_id(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1] & 0x01;
    return 2;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.17, page 51
 * Type = 20 (Decimal)
 */
static u32 decode_gtp_nsapi(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1] & 0x0F;
    return 2;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.18, page 52
 * Type = 21 (Decimal)
 */
static u32 decode_gtp_ranap_cause(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1];
    return 2;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.19, page 52
 * Type = 22 (Decimal)
 */
static u32 decode_gtp_rab_cntxt(u8 *tv, void *out)
{
    struct RAB_CNTXT *rab = (struct RAB_CNTXT*)out;

    rab->nsapi = tv[1] & 0x0F;
    rab->rab_gtpu_dn = *(u16 *)(tv + 2);
    rab->rab_gtpu_up = *(u16 *)(tv + 4);
    rab->rab_pdu_dn  = *(u16 *)(tv + 6);
    rab->rab_pdu_up  = *(u16 *)(tv + 8);
    
    return 10;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.20, page 53
 * Type = 23 (Decimal)
 */
static u32 decode_gtp_rp_sms(u8 *tv, void *out)
{
    *((u8 *)out) = tv[1] & 0x07;
    return 2;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.21, page 53
 * Type = 24 (Decimal)
 */
static u32 decode_gtp_rp(u8 *tv, void *out)
{
    struct RD_PRI *rp = (struct RD_PRI*)out;
    rp->nsapi = tv[1] & 0xF0 >> 4;
    rp->radio_priority = tv[1] &0x07;
    return 2;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.22, page 53
 * Type = 25 (Decimal)
 */
static u32 decode_gtp_pkt_flow_id(u8 *tv, void *out)
{
    struct FLOW_ID *flow = (struct FLOW_ID*)out;
    flow->nsapi = tv[1] & 0x0F;
    flow->flow_id = tv[2];
    return 3;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.23, page 53
 * Type = 26 (Decimal)
 */
static u32 decode_gtp_chrg_char(u8 *tv, void *out)
{
    *(u16 *)out = *(u16 *)(tv + 1);
    return 3;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.24, page
 * Type = 27 (Decimal)
 */
static u32 decode_gtp_trace_ref(u8 *tv, void *out)
{
    *(u16 *)out = *(u16 *)(tv + 1);
    return 3;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.25, page
 * Type = 28 (Decimal)
 */
static u32 decode_gtp_trace_type(u8 *tv, void *out)
{
    *(u16 *)out = *(u16 *)(tv + 1);
    return 3;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.16A
 * UMTS:    29.060 v4.0, chapter 7.7.25A, page
 * Type = 29 (Decimal)
 */
static u32 decode_gtp_ms_reason(u8 *tv, void *out)
{
    *(u8 *)out = *(u8 *)(tv + 1);
    return 2;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.25B
 * Radio Priority LCS
 * Type = 150 (Decimal)
 */
static u32 decode_gtp_ra_prio_lcs(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    12.15 v7.6.0, chapter 7.3.3, page 45
 * UMTS:    33.015
 */
static u32 decode_gtp_tr_comm(u8 *tv, void *out)
{
    *(u8 *)out = tv[1];
    return 2;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.17, page 43
 * UMTS:    29.060 v4.0, chapter 7.7.26, page 55
 * Type = 127 (Decimal)
 */
static u32 decode_gtp_chrg_id(u8 *tv, void *out)
{
    *(u32 *)out = ntohl(*(u32 *)(tv + 1));
    return 5;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.18, page 43
 * UMTS:    29.060 v4.0, chapter 7.7.27, page 55
 * Type = 128 (Decimal)
 */
static u32 decode_gtp_user_addr(u8 *tv, void *out)
{
    u16 length;
    u8  pdp_typ, pdp_org;
    struct IPADDR *addr = (struct IPADDR*)out;
  
    length = ntohs(*(u16 *)(tv + 1));
    
    pdp_org = tv[3] & 0x0F;
    pdp_typ = tv[4];

    if (length == 2) {
        /*FIX ME*/
        if (pdp_org == 0 && pdp_typ == 1)
            addr->len = 0;
        else if (pdp_typ == 2)
            addr->len = 0;

    } else if (length > 2) {

        switch (pdp_typ) {
        case 0x21:
            addr->len = 4;
            addr->addr4 = *(u32 *)(tv + 5);
        break;
        case 0x57:
            addr->len = 16;
            memcpy(&addr->addr6, (tv + 5), 16);
        break;
        }
    } else 
        addr->len = 0;

    return 3 + length;
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.19 page
 * UMTS:    29.060 v4.0, chapter 7.7.28 page 57
 * Type = 129 (Decimal)
 */
static u32 decode_gtp_mm_cntxt(u8 *tv, void *out)
{
    return 3 + ntohs(*(u16 *)(tv + 1));
}


static void decode_apn(u8 *buf, u16 length, char *out)
{
    u32 name_len, tmp;
    
    if (length > 0) {
        name_len = buf[0];

        if (name_len < 0x20) {
            memcpy(out, buf + 1, length-1);
            for(;;) {
                if(name_len >= length-1)
                    break;
                tmp = out[name_len];
                out[name_len] = '.';
                name_len = name_len + tmp + 1;
            }
        } else
            memcpy(out, buf, length);
    }
}


/* GPRS:    9.60 v7.6.0, chapter 7.9.20
 * UMTS:    29.060 v4.0, chapter 7.7.29
 * Type = 130 (Decimal)
 */
static u32 decode_gtp_pdp_cntxt(u8 *tv, void *out)
{
    u8  qos_sub_len, qos_req_len,
        qos_neg_len, ggsn_addr_len, 
        pdp_type_num, pdp_addr_len, apn_len;

    u8  *buf = tv + 3;
    struct PDP_CNTXT *cnt = (struct PDP_CNTXT*)out; 

    u16 length = ntohs(*(u16 *)(tv + 1));

    /*oct4*/
    cnt->ea  = (*buf >> 7) & 0x01;
    cnt->vaa = (*buf >> 6) & 0x01;
    cnt->asi = (*buf >> 5) & 0x01;
    cnt->order = (*buf >> 4) & 0x01;
    cnt->nsapi = *buf & 0x0F;
    buf++;    

    /*oct5*/
    cnt->sapi = (*buf) & 0x0F;
    buf++;
    
    /*FIX ME, if do something about qos*/
    qos_sub_len = *buf; 
    buf += qos_sub_len + 1;

    qos_req_len = *buf;
    buf += qos_req_len + 1;

    qos_neg_len = *buf;
    buf += qos_neg_len + 1;

    cnt->sn_down = ntohs(*(u16 *)buf);
    cnt->sn_up   = ntohs(*(u16 *)(buf + 2));
    buf += 4;

    cnt->pdu_send_no = *(u8 *)(buf);
    cnt->pdu_rec_no  = *(u8 *)(buf + 1);
    buf += 2;

    cnt->up_teid_control = *(u32 *)buf;
    cnt->up_teid_data1 = *(u32 *)(buf + 4);
    buf += 8;
    
    cnt->pdp_cntxt_id = *(u8 *)buf;
    buf += 1;

    cnt->pdp_type_org = *(u8 *)buf & 0x0F;
    buf += 1;

    pdp_type_num = *(u8 *)buf;
    buf += 1;

    pdp_addr_len = *(u8 *)buf;
    buf += 1;

    /*get pdp address*/
    if (pdp_addr_len > 0) {
        switch (pdp_type_num) {

        case 0x21:
            cnt->pdp_addr.len = 4;
            cnt->pdp_addr.addr4 = *(u32 *)(buf);
            break;
        
        case 0x57:
            cnt->pdp_addr.len = 16;
            memcpy(&cnt->pdp_addr.addr6, buf, 16);
            break;

        default:
            cnt->pdp_addr.len = 0;
            break;
        }
    }
    
    buf += pdp_addr_len;

    ggsn_addr_len = *buf;
    buf++;

    switch (ggsn_addr_len) {
    case 4:
        cnt->ggsn_addr_ctl.len = 4;
        cnt->ggsn_addr_ctl.addr4 = *(u32 *)buf;
        break;

    case 16:
        cnt->ggsn_addr_ctl.len = 16;
        memcpy(&cnt->ggsn_addr_ctl.addr6, buf, 16);
        break;
    default:
        cnt->ggsn_addr_ctl.len = 0;
        break;
    }

    buf += ggsn_addr_len;

    ggsn_addr_len = *buf;
    buf++;
    
    switch (ggsn_addr_len) {
    case 4:
        cnt->ggsn_addr_data.len = 4;
        cnt->ggsn_addr_data.addr4 = *(u32 *)buf;
        break;
    
    case 16:
        cnt->ggsn_addr_data.len = 16;
        memcpy(&cnt->ggsn_addr_data.addr6, buf, 16);
        break;

    default:
         cnt->ggsn_addr_data.len = 0;
        break;
    }
    buf += ggsn_addr_len;

    apn_len = *(u8 *)buf;
    buf++;

    decode_apn(buf, apn_len, cnt->apn);
    buf += apn_len;
 
    cnt->trans_id = *(u8 *)buf;
    /*FIX ME !!!!*/
    return 3 + length;
}

/* GPRS:    9.60, v7.6.0, chapter 7.9.21
 * UMTS:    29.060, v4.0, chapter 7.7.30
 * Type = 131 (Decimal)
 */
static u32 decode_gtp_apn(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));

    decode_apn(tv + 3, length, out);
    return 3 + length;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.22
 *         4.08 v. 7.1.2, chapter 10.5.6.3 (p.580)
 * UMTS:    29.060 v4.0, chapter 7.7.31
 *         24.008, v4.2, chapter 10.5.6.3
 * Type = 132 (Decimal)
 */
static u32 decode_gtp_proto_conf(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.23
 * UMTS:    29.060 v4.0, chapter 7.7.32
 * Type = 133 (Decimal)
 */
static u32 decode_gtp_gsn_addr(u8 *tv, void *out)
{
    u16 length;
    struct IPADDR *addr = (struct IPADDR*)out; 
    length = ntohs(*(u16 *)(tv + 1));
    
    switch (length) {
    case 4:
        addr->len = 4;
        addr->addr4 = *(u32 *)(tv + 3);
        break;
    case 5:
        addr->len = *(u8 *)(tv + 3) & 0x3F;
        addr->addr4 = *(u32 *)(tv + 4);
        break;
    case 16:
        addr->len = 16;
        memcpy(&addr->addr4, tv + 4 , 16);
    break;
    case 17:
        addr->len = *(u8 *)(tv + 3) & 0x3F;
        memcpy(&addr->addr4, tv + 4 , addr->len);
        break;
    default:
        addr->len = 0;
        break;
    }

    return 3 + length;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.24
 * UMTS:    29.060 v4.0, chapter 7.7.33
 * Type = 134 (Decimal)
 */
static u32 decode_gtp_msisdn(u8 *tv, void *out)
{
    u16 length;
    struct msisdn *msisdn = (struct msisdn *)out;
    length = ntohs(*(u16 *)(tv + 1));

    if (length > 1) { 
        msisdn_to_str((tv+3), length, msisdn->str);
        msisdn->dig = strtoll(msisdn->str, NULL, 10);    
        // *(u64 *)out = msisdn_to_ull(tv + 3, length);
    } else {
        msisdn->dig = msisdn->str[0] = 0; 
        //*(u64 *)out = 0;
    }
    return 3 + length;
}

static int decode_qos_umts(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    u8  max_sdu_size, max_ul, max_dl;
	struct UMTS_QOS *qos = (struct UMTS_QOS *)out;

   	u8 *offset = tv + 3;
    if (length > 4) {
        max_sdu_size = *(offset + (5 - 1) + 1);
	    max_ul = *(offset + (6 - 1) + 1);
	    max_dl = *(offset + (7 - 1) + 1);

	    if (max_sdu_size == 0 || max_sdu_size > 150)
	        qos->mss = max_sdu_size;
        else if (max_sdu_size > 0 && max_sdu_size <= 150)
	        qos->mss = max_sdu_size * 10;

        if (max_ul == 0 || max_ul == 255)
            qos->mu = max_ul;
		else if (max_ul > 0 && max_ul <= 63)
            qos->mu = max_ul;
	    else if (max_ul > 63 && max_ul <= 127) 
            qos->mu = 64 + (max_ul - 64) * 8;
        else if (max_ul > 127 && max_ul <= 254)
            qos->mu = 576 + (max_ul - 128) * 64;

        if (max_dl == 0 || max_dl == 255)
            qos->md = max_dl;
        else if (max_dl > 0 && max_dl <= 63)
            qos->md = max_dl;
        else if (max_dl > 63 && max_dl <= 127)
            qos->md = 64 + (max_dl - 64) * 8;
        else if (max_dl > 127 && max_dl <= 254)
            qos->md = 576 + (max_dl - 128) * 64;
    }

    return 3 + length;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.34
 *         24.008 v4.2, chapter 10.5.6.5
 * Type = 135 (Decimal)
 */
static u32 decode_gtp_qos_umts(u8 *tv, void *out)
{
    return decode_qos_umts(tv, out);
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.35
 * Type = 136 (Decimal)
 */
static u32 decode_gtp_auth_qui(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.36
 *         24.008 v4.2, chapter 10.5.6.12
 * Type = 137 (Decimal)
 */
static u32 decode_gtp_tft(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.37
 * Type = 138 (Decimal)
 */
static u32 decode_gtp_target_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.38
 * Type = 139 (Decimal)
 */
static u32 decode_gtp_utran_cont(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.39
 * Type = 140 (Decimal)
 */
static u32 decode_gtp_rab_setup(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}


/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.40
 * Type = 141 (Decimal)
 */
static u32 decode_gtp_hdr_list(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.41
 * Type = 142 (Decimal)
 */
static u32 decode_gtp_trigger_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    not present
 * UMTS:    29.060 v4.0, chapter 7.7.42
 * Type = 143 (Decimal)
 */
static u32 decode_gtp_omc_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.25
 * UMTS:    29.060 v6.11.0, chapter 7.7.44 Charging Gateway Address
 * Type = 251 (Decimal)
 */
static u32 decode_gtp_chrg_addr(u8 *tv, void *out)
{
    u16 length;
    struct IPADDR *addr = (struct IPADDR*)out; 

    length = ntohs(*(u16 *)(tv + 1));
    switch (length) {
    case 4:
        addr->len = 4;
        addr->addr4 = *(u32 *)(tv + 3);
        break;

    case 16:
        addr->len = 16;
        memcpy(&addr->addr4, tv + 4 , 16);
        break;
    
    default:
        addr->len = 0;
        break;
    }
       
    return 3 + length;
}


/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.44 RAN Transparent Container
 * Type = 145 (Decimal)
 */
static u32 decode_gtp_ran_tr_cont(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.45 PDP Context Prioritization
 * Type = 146 (Decimal)
 */
static u32 decode_gtp_pdp_cont_prio(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.45A Additional RAB Setup Information
 */
static u32 decode_gtp_add_rab_setup_inf(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}


 /* GPRS:       ?
  * UMTS:       29.060 v6.11.0, chapter 7.7.47 SGSN Number
  * Type = 147 (Decimal) 
  */
static u32 decode_gtp_ssgn_no(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    3GPP TS 29.060 version 7.8.0 Release 7, chapter 7.7.48 Common Flags
 * Type = 148 (Decimal) 
 */
static u32 decode_gtp_common_flgs(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.49
 * Type = 149 (Decimal) 
 */
static u32 decode_gtp_apn_res(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.50
 * RAT Type
 * Type = 151 (Decimal)
 */

static u32 decode_gtp_rat_type(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    *(u8 *)out = tv[3];
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.51
 * User Location Information
 * Type = 152 (Decimal)
 */
static u32 decode_gtp_usr_loc_inf(u8 *tv, void *out)
{
    u16 length, mnc, mcc;
    u8  byte[3];
    struct USR_LOC *usr_loc = (struct USR_LOC*)out;

    length = ntohs(*(u16 *)(tv + 1));
    /* Geographic Location Type */
    usr_loc->type = *(u8 *)(tv + 3);
    
    byte[0] = tv[4];
    byte[1] = tv[5];
    byte[2] = tv[6];
       
    mcc = (byte[0] & 0x0F) * 100 + ((byte[0] & 0xF0) >> 4) * 10 + (byte[1] & 0x0F);
    if ((byte[1] & 0xF0) == 0xF0)
        mnc = (byte[2] & 0x0F) * 10 + ((byte[2] & 0xF0) >> 4);
    else
        mnc = (byte[2] & 0x0F) * 100 + ((byte[2] & 0xF0) >> 4) * 10 + ((byte[1] & 0xF0) >> 4);

    if (usr_loc->type == 0) {
        usr_loc->cgi.mcc = mcc;
        usr_loc->cgi.mnc = mnc;
        usr_loc->cgi.lac =  ntohs(*(u16 *)(tv + 7));
        usr_loc->cgi.ci  =  ntohs(*(u16 *)(tv + 9));
    } else if (usr_loc->type == 1) {
        usr_loc->sai.mcc = mcc;
        usr_loc->sai.mnc = mnc;
        usr_loc->sai.lac = ntohs(*(u16 *)(tv + 7));
        usr_loc->sai.sac = ntohs(*(u16 *)(tv + 9));
    } else {
        usr_loc->rai.mcc = mcc;
        usr_loc->rai.mnc = mnc;
        usr_loc->rai.lac =  ntohs(*(u16 *)(tv + 7));
        usr_loc->rai.rac =  ntohs(*(u16 *)(tv + 9));
    }
    
    return 3 + length;
}


/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.52
 * MS Time Zone
 * Type = 153 (Decimal)
 */
static u32 decode_gtp_ms_time_zone(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    struct MS_TMZ *tz = (struct MS_TMZ*)out;

    tz->tm_zone = tv[3];
    tz->daylight = tv[4] & 0x10;

    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.53
 * International Mobile Equipment Identity (and Software Version) (IMEI(SV))
 * Type = 154 (Decimal)
 */
static u32 decode_gtp_imeisv(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    struct imei *imei = (struct imei *)out;

    if (length <= 9) {
        digital_to_str((tv + 3), length, imei->str);
        imei->dig = strtoll(imei->str, NULL, 10);
    } else 
        imei->dig = imei->str[0] = 0;

    /* IMEI(SV)*/
   // *(u64 *)out = digital_to_ull(tv + 3, length);
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.54
 * CAMEL Charging Information Container
 * Type = 155 (Decimal)
 */
static u32 decode_gtp_camel_chg_inf_con(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.55
 * MBMS UE Context
 * Type = 156 (Decimal)
 */
static u32 decode_gtp_mbms_ue_ctx(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    3GPP TS 29.060 version 7.8.0 Release 7, chapter 7.7.56
 * Temporary Mobile Group Identity (TMGI)
 * Type = 157 (Decimal)
 */

static u32 decode_gtp_tmgi(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.57
 * RIM Routing Address
 * Type = 158 (Decimal)
 */
static u32 decode_gtp_rim_ra(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.58
 * MBMS Protocol Configuration Options
 * Type = 159 (Decimal)
 */
static u32 decode_gtp_mbms_prot_conf_opt(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    3GPP TS 29.060 version 7.8.0 Release 7, chapter 7.7.59
 * MBMS Session Duration
 * Type = 168 (Decimal)
 */
static u32 decode_gtp_mbms_ses_dur(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    3GPP TS 29.060 version 7.8.0 Release 7, chapter 7.7.60
 * MBMS Service Area
 * Type = 160 (Decimal)
 */
static u32 decode_gtp_mbms_sa(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.61
 * Source RNC PDCP context info
 * Type = 161 (Decimal)
 */
static u32 decode_gtp_src_rnc_pdp_ctx_inf(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.62
 * Additional Trace Info
 * Type = 162 (Decimal)
 */
static u32 decode_gtp_add_trs_inf(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.63
 * Hop Counter
 * Type = 163 (Decimal)
 */
static u32 decode_gtp_hop_count(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.64
 * Selected PLMN ID
 * Type = 164 (Decimal)
 */
static u32 decode_gtp_sel_plmn_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.65
 * MBMS Session Identifier
 * Type = 165 (Decimal)
 */
static u32 decode_gtp_mbms_ses_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.66
 * MBMS 2G/3G Indicator
 * Type = 166 (Decimal)
 */

static u32 decode_gtp_mbms_2g_3g_ind(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.67
 * Enhanced NSAPI
 * Type = 167 (Decimal)
 */
static u32 decode_gtp_enh_nsapi(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.68
 * Additional MBMS Trace Info
 * Type = 169 (Decimal)
 */
static u32 decode_gtp_add_mbms_trs_inf(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.69
 * MBMS Session Identity Repetition Number
 * Type = 170 (Decimal)
 */
static u32 decode_gtp_mbms_ses_id_rep_no(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    3GPP TS 29.060 version 7.8.0 Release 7
 * MBMS Time To Data Transfer
 * Type = 171 (Decimal)
 */
static u32 decode_gtp_mbms_time_to_data_tr(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.71
 * PS Handover Request Context 
 * Type = 172 (Decimal)
 */
static u32 decode_gtp_ps_ho_req_ctx(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.72
 * BSS Container 
 * Type = 173 (Decimal)
 */
static u32 decode_gtp_bss_cont(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.73
 * Cell Identification 
 * Type = 174 (Decimal)
 */
static u32  decode_gtp_cell_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.74
 * PDU Numbers
 * Type = 175 (Decimal)
 */
static u32  decode_gtp_pdu_no(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    ?
 * UMTS:    29.060 v6.11.0, chapter 7.7.75
 * BSSGP Cause
 * Type = 176 (Decimal)
 */
static u32 decode_gtp_bssgp_cause(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/*
 * Required MBMS bearer capabilities    7.7.76
 * Type = 177 (Decimal)
 */
static u32 decode_gtp_mbms_bearer_cap(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/*
 * RIM Routing Address Discriminator    7.7.77
 * Type = 178 (Decimal)
 */
static u32 decode_gtp_rim_ra_disc(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/*
 * List of set-up PFCs    7.7.78
 * Type = 179 (Decimal)
 */
static u32 decode_gtp_lst_set_up_pfc(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}
/*
 * PS Handover XID Parameters    7.7.79
 * Type = 180 (Decimal)
 */
static u32 decode_gtp_ps_handover_xid(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/*
 * MS Info Change Reporting Action    7.7.80
 * Type = 181 (Decimal)
 */
static u32 decode_gtp_ms_inf_chg_rep_act(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}
/*
 * Direct Tunnel Flags    7.7.81
 * Type = 182 (Decimal)
 */
static u32 decode_gtp_direct_tnl_flg(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}
/*
 * Correlation-ID    7.7.82
 * Type = 183 (Decimal)
 */
static u32 decode_gtp_corrl_id(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}
/*
 * Bearer Control Mode    7.7.83
 * Type = 184 (Decimal)
 */
static u32 decode_gtp_bearer_cntrl_mod(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}
/* GPRS:    12.15
 * UMTS:    33.015
 */
static u32 decode_gtp_rel_pack(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    12.15
 * UMTS:    33.015
 */
static u32 decode_gtp_can_pack(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* CDRs dissector */
static u32 decode_gtp_data_req(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    12.15
 * UMTS:    33.015
 */
static u32 decode_gtp_data_resp(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}

/* GPRS:    12.15
 * UMTS:    33.015
 */
static u32 decode_gtp_node_addr(u8 *tv, void *out)
{
    u16 length;
    struct IPADDR *addr = (struct IPADDR *)out; 
    length = ntohs(*(u16 *)(tv + 1));
    
    switch (length) {
    case 4:
        addr->len = 4;
        addr->addr4 = *(u32 *)(tv + 3);
        break;

    case 16:
        addr->len = 16;
        memcpy(&addr->addr4, tv + 4 , 16);
        break;

    default:
        addr->len = 0;
        break;
    }
    
    return 3 + length;
}

/* GPRS:    9.60 v7.6.0, chapter 7.9.26
 * UMTS:    29.060 v4.0, chapter 7.7.46 Private Extension
 *
 */
static u32 decode_gtp_priv_ext(u8 *tv, void *out)
{
    u16 length = ntohs(*(u16 *)(tv + 1));
    return 3 + length;
}


typedef struct _gtp_opt {
    u32 optcode;
    u32 (*decode)(u8 *, void*);
    u32  len;
} gtp_opt_t;


static const gtp_opt_t _gtpopt[] = {
    /*TV*/
    {GTP_EXT_CAUSE, decode_gtp_cause, 2},
    {GTP_EXT_IMSI, decode_gtp_imsi, 9},
    {GTP_EXT_RAI, decode_gtp_rai, 7},
    {GTP_EXT_TLLI, decode_gtp_tlli, 5},
    {GTP_EXT_PTMSI, decode_gtp_ptmsi, 5},
    {GTP_EXT_QOS_GPRS, decode_gtp_qos_gprs, 4},
    {GTP_EXT_REORDER, decode_gtp_reorder, 2},
    {GTP_EXT_AUTH_TRI, decode_gtp_auth_tri, 29},
    {GTP_EXT_MAP_CAUSE, decode_gtp_map_cause, 2},
    {GTP_EXT_PTMSI_SIG, decode_gtp_ptmsi_sig, 4},
    {GTP_EXT_MS_VALID, decode_gtp_ms_valid, 2},
    {GTP_EXT_RECOVER, decode_gtp_recovery, 2},
    {GTP_EXT_SEL_MODE, decode_gtp_sel_mode, 2},
    {GTP_EXT_TEID, decode_gtp_teid_data1, 5},
    {GTP_EXT_TEID_CP, decode_gtp_teid_cp, 5},
    {GTP_EXT_18, decode_gtp_teid_data2, 6},
    {GTP_EXT_19, decode_gtp_tear_id, 2},
    {GTP_EXT_NSAPI, decode_gtp_nsapi, 2},
    {GTP_EXT_RANAP_CAUSE, decode_gtp_ranap_cause, 2},
    {GTP_EXT_RAB_CNTXT, decode_gtp_rab_cntxt, 10},
    {GTP_EXT_RP_SMS, decode_gtp_rp_sms, 2},
    {GTP_EXT_RP, decode_gtp_rp, 2},
    {GTP_EXT_PKT_FLOW_ID, decode_gtp_pkt_flow_id, 3},
    {GTP_EXT_CHRG_CHAR, decode_gtp_chrg_char, 3},
    {GTP_EXT_TRACE_REF, decode_gtp_trace_ref, 3},
    {GTP_EXT_TRACE_TYPE, decode_gtp_trace_type, 3},
    {GTPv1_EXT_MS_REASON, decode_gtp_ms_reason, 2},
    {GTP_EXT_TR_COMM, decode_gtp_tr_comm, 4},
    {GTP_EXT_CHRG_ID, decode_gtp_chrg_id, 5},

    /*TLV*/
    {GTP_EXT_USER_ADDR, decode_gtp_user_addr},
    {GTP_EXT_MM_CNTXT, decode_gtp_mm_cntxt},
    {GTP_EXT_PDP_CNTXT, decode_gtp_pdp_cntxt},
    {GTP_EXT_APN, decode_gtp_apn},
    {GTP_EXT_PROTO_CONF, decode_gtp_proto_conf},
    {GTP_EXT_GSN_ADDR, decode_gtp_gsn_addr},
    {GTP_EXT_MSISDN, decode_gtp_msisdn},
    {GTP_EXT_QOS_UMTS, decode_gtp_qos_umts},    /* 3G */
    {GTP_EXT_AUTH_QUI, decode_gtp_auth_qui},    /* 3G */
    {GTP_EXT_TFT, decode_gtp_tft},    /* 3G */
    {GTP_EXT_TARGET_ID, decode_gtp_target_id},    /* 3G */
    {GTP_EXT_UTRAN_CONT, decode_gtp_utran_cont},    /* 3G */
    {GTP_EXT_RAB_SETUP, decode_gtp_rab_setup},    /* 3G */
    {GTP_EXT_HDR_LIST, decode_gtp_hdr_list},    /* 3G */
    {GTP_EXT_TRIGGER_ID, decode_gtp_trigger_id},    /* 3G */
    {GTP_EXT_OMC_ID, decode_gtp_omc_id},    /* 3G */
    /* TS 29 060 V6.11.0 */
    {GTP_EXT_RAN_TR_CONT, decode_gtp_ran_tr_cont},    /* 7.7.43 */
    {GTP_EXT_PDP_CONT_PRIO, decode_gtp_pdp_cont_prio},    /* 7.7.45 */
    {GTP_EXT_ADD_RAB_SETUP_INF, decode_gtp_add_rab_setup_inf},    /* 7.7.45A */
    {GTP_EXT_SSGN_NO, decode_gtp_ssgn_no},    /* 7.7.47 */
    {GTP_EXT_COMMON_FLGS, decode_gtp_common_flgs},    /* 7.7.48 */
    {GTP_EXT_APN_RES, decode_gtp_apn_res},    /* 3G */
    {GTP_EXT_RA_PRIO_LCS, decode_gtp_ra_prio_lcs},    /* 7.7.25B */
    {GTP_EXT_RAT_TYPE, decode_gtp_rat_type},    /* 3G */
    {GTP_EXT_USR_LOC_INF, decode_gtp_usr_loc_inf},    /* 7.7.51 */
    {GTP_EXT_MS_TIME_ZONE, decode_gtp_ms_time_zone},    /* 7.7.52 */
    {GTP_EXT_IMEISV, decode_gtp_imeisv},    /* 3G 7.7.53 */
    {GTP_EXT_CAMEL_CHG_INF_CON, decode_gtp_camel_chg_inf_con},    /* 7.7.54 */
    {GTP_EXT_MBMS_UE_CTX, decode_gtp_mbms_ue_ctx},    /* 7.7.55 */
    {GTP_EXT_TMGI, decode_gtp_tmgi},    /* 7.7.56 */
    {GTP_EXT_RIM_RA, decode_gtp_rim_ra},    /* 7.7.57 */
    {GTP_EXT_MBMS_PROT_CONF_OPT, decode_gtp_mbms_prot_conf_opt},    /* 7.7.58 */
    {GTP_EXT_MBMS_SA, decode_gtp_mbms_sa},    /* 7.7.60 */
    {GTP_EXT_SRC_RNC_PDP_CTX_INF, decode_gtp_src_rnc_pdp_ctx_inf},    /* 7.7.61 */
    {GTP_EXT_ADD_TRS_INF, decode_gtp_add_trs_inf},    /* 7.7.62 */
    {GTP_EXT_HOP_COUNT, decode_gtp_hop_count},    /* 7.7.63 */
    {GTP_EXT_SEL_PLMN_ID, decode_gtp_sel_plmn_id},    /* 7.7.64 */
    {GTP_EXT_MBMS_SES_ID, decode_gtp_mbms_ses_id},    /* 7.7.65 */
    {GTP_EXT_MBMS_2G_3G_IND, decode_gtp_mbms_2g_3g_ind},    /* 7.7.66 */
    {GTP_EXT_ENH_NSAPI, decode_gtp_enh_nsapi},    /* 7.7.67 */
    {GTP_EXT_MBMS_SES_DUR, decode_gtp_mbms_ses_dur},    /* 7.7.59 */
    {GTP_EXT_ADD_MBMS_TRS_INF, decode_gtp_add_mbms_trs_inf},    /* 7.7.68 */
    {GTP_EXT_MBMS_SES_ID_REP_NO, decode_gtp_mbms_ses_id_rep_no},    /* 7.7.69 */
    {GTP_EXT_MBMS_TIME_TO_DATA_TR, decode_gtp_mbms_time_to_data_tr},    /* 7.7.70 */
    {GTP_EXT_PS_HO_REQ_CTX, decode_gtp_ps_ho_req_ctx},    /* 7.7.71 */
    {GTP_EXT_BSS_CONT, decode_gtp_bss_cont},    /* 7.7.72 */
    {GTP_EXT_CELL_ID, decode_gtp_cell_id},    /* 7.7.73 */
    {GTP_EXT_PDU_NO, decode_gtp_pdu_no},    /* 7.7.74 */
    {GTP_EXT_BSSGP_CAUSE, decode_gtp_bssgp_cause},    /* 7.7.75 */
    {GTP_EXT_REQ_MBMS_BEARER_CAP, decode_gtp_mbms_bearer_cap},    /* 7.7.76 */
    {GTP_EXT_RIM_ROUTING_ADDR_DISC, decode_gtp_rim_ra_disc},    /* 7.7.77 */
    {GTP_EXT_LIST_OF_SETUP_PFCS, decode_gtp_lst_set_up_pfc},    /* 7.7.78 */
    {GTP_EXT_PS_HANDOVER_XIP_PAR, decode_gtp_ps_handover_xid},    /* 7.7.79 */
    {GTP_EXT_MS_INF_CHG_REP_ACT, decode_gtp_ms_inf_chg_rep_act},    /* 7.7.80 */
    {GTP_EXT_DIRECT_TUNNEL_FLGS, decode_gtp_direct_tnl_flg},    /* 7.7.81 */
    {GTP_EXT_CORRELATION_ID, decode_gtp_corrl_id},    /* 7.7.82 */
    {GTP_EXT_BEARER_CONTROL_MODE, decode_gtp_bearer_cntrl_mod},    /* 7.7.83 */

    {GTP_EXT_REL_PACK, decode_gtp_rel_pack},    /* charging */
    {GTP_EXT_CAN_PACK, decode_gtp_can_pack},    /* charging */
    {GTP_EXT_CHRG_ADDR, decode_gtp_chrg_addr},
    {GTP_EXT_DATA_REQ, decode_gtp_data_req},    /* charging */
    {GTP_EXT_DATA_RESP, decode_gtp_data_resp},    /* charging */
    {GTP_EXT_NODE_ADDR, decode_gtp_node_addr},
    {GTP_EXT_PRIV_EXT, decode_gtp_priv_ext},
};

struct gtp_opt{
    u32 (*decode)(u8 *, void *);
    u32  len;
};

static struct gtp_opt *options;
const int opt_known = sizeof(_gtpopt)/sizeof(_gtpopt[0]);

int gtp_opt_init()
{
    int i = 0, optcode;
        options = calloc(256, sizeof(*options));
    if (!options)
        return -1;

    for (i = 0; i < opt_known; i++){
        optcode = _gtpopt[i].optcode;
        if (optcode > 255 || options[optcode].decode)
            break;

        options[optcode].decode = _gtpopt[i].decode;
        options[optcode].len    = _gtpopt[i].len;
    }

    if (i < opt_known) {
        printf("#error gtp options at _gtpopt[%u], optcode = %d\n", i, optcode);    
        free(options);
        options = NULL;
        return -1;
    }
    return 0;
}

void gtp_opt_clean()
{
    if (options) {
        free(options);
        options = NULL; 
    }
}


void dump_gtp_pdp_create_request(struct pdp_create_req *req)
{
    GTP_DEBUG("pdp_create_request imsi:%llu msisdn:%llu imeisv:%llu "
              "teid_data:0x%08x teid_cp:0x%08x sel_mod:%u "
              "nsapi:%u naspi_link:%u rat_type:%u user_addr:%d.%d.%d.%d "
              "apn:%s sgsn_addr_cp:%d.%d.%d.%d sgsn_addr_data:%d.%d.%d.%d\n",
              req->imsi.dig, req->msisdn.dig, req->imeisv.dig,
              req->teid_data, req->teid_cp, req->sel_mod,
              req->nsapi, req->nsapi_link, req->rat_type, _ADDR(req->usr_addr.addr4),
              req->apn, _ADDR(req->gsn_addr_cp.addr4), _ADDR(req->gsn_addr_data.addr4));
}

int gtp_do_create_pdp_request(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct  gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct  pdp_create_req *request = (struct pdp_create_req *)gtpinfo->data;

    memset(request, 0, sizeof(struct pdp_create_req));
    end = tv + gtpinfo->payload_len;
    
    while (tv < end) {
        optcode = *tv;
        
        switch(optcode) {
        case GTP_EXT_IMSI:
            tv += decode_gtp_imsi(tv, &request->imsi);
            break;

        case GTP_EXT_RAI:
            tv += decode_gtp_rai(tv, &request->rai);
            break;

		case GTP_EXT_QOS_GPRS:
            tv += decode_gtp_qos_gprs(tv, &request->gprs_qos);
            break;
		
        case GTP_EXT_SEL_MODE:
            tv += decode_gtp_sel_mode(tv, &request->sel_mod);
            break;
        
        case GTP_EXT_TEID:
            tv += decode_gtp_teid_data1(tv, &request->teid_data);
            break;
        
        case GTP_EXT_TEID_CP:
            tv += decode_gtp_teid_cp(tv, &request->teid_cp);
            break;
        
        case GTP_EXT_NSAPI:
            if (request->nsapi == 0)
                tv += decode_gtp_nsapi(tv, &request->nsapi);
            else
                tv += decode_gtp_nsapi(tv, &request->nsapi_link);
            break;
        
        case GTP_EXT_USER_ADDR:
            tv += decode_gtp_user_addr(tv, &request->usr_addr);
            break;

        case GTP_EXT_APN:
            tv += decode_gtp_apn(tv, request->apn);
            break;
        
        case GTP_EXT_GSN_ADDR:
            if (request->gsn_addr_cp.len == 0)
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_cp);
            else
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_data);
            break;

        case GTP_EXT_RAT_TYPE:
            tv += decode_gtp_rat_type(tv, &request->rat_type);
            break;

		case GTP_EXT_QOS_UMTS:
            tv += decode_gtp_qos_umts(tv, &request->umts_qos);
			break;
        
        case GTP_EXT_MSISDN:
            tv += decode_gtp_msisdn(tv, &request->msisdn);
            break;
 
        case GTP_EXT_IMEISV:
            tv += decode_gtp_imeisv(tv, &request->imeisv);
            break;

        case GTP_EXT_USR_LOC_INF:
            tv += decode_gtp_usr_loc_inf(tv, &request->location);
			break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                   /*FIX ME!!! log and quit*/
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            }
            else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));

            break;
        }
    }
    dump_gtp_pdp_create_request(request);
    return  pdp_do_create_request(pkb);

    /* insert to pdp_context database */
}



void dump_gtp_pdp_create_response(struct pdp_create_resp *rsp)
{
    GTP_DEBUG("pdp_create_response cause:%u "
              "teid_data:0x%08x teid_cp:0x%08x chrg_id:0x%08x "
              "nsapi:%u user_addr:%d.%d.%d.%d "
              "ggsn_addr_cp:%d.%d.%d.%d ggsn_addr_data:%d.%d.%d.%d\n",
              rsp->cause, rsp->teid_data, rsp->teid_cp, rsp->chrg_id,
              rsp->nsapi, _ADDR(rsp->usr_addr.addr4),
              _ADDR(rsp->gsn_addr_cp.addr4), _ADDR(rsp->gsn_addr_data.addr4));
}


int gtp_do_create_pdp_response(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct  gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct  pdp_create_resp *response = (struct pdp_create_resp *)gtpinfo->data;

    memset(response, 0, sizeof(struct pdp_create_resp));
    end = tv + gtpinfo->payload_len;

    while (tv < end) {
        optcode = *tv;

        switch(optcode) {
        case GTP_EXT_CAUSE:
            tv += decode_gtp_cause(tv, &response->cause);
            break;

        case GTP_EXT_TEID:
            tv += decode_gtp_teid_data1(tv, &response->teid_data);
            break;
    
        case GTP_EXT_TEID_CP:
            tv += decode_gtp_teid_cp(tv, &response->teid_cp);
            break;
    
        case GTP_EXT_NSAPI:
            tv += decode_gtp_nsapi(tv, &response->nsapi);
            break;
    
        case GTP_EXT_CHRG_ID:
            tv += decode_gtp_chrg_id(tv, &response->chrg_id);
            break;
    
        case GTP_EXT_USER_ADDR:
            tv += decode_gtp_user_addr(tv, &response->usr_addr);
            break;

        case GTP_EXT_GSN_ADDR:
            if (response->gsn_addr_cp.len == 0)
                tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_cp);
            else
                tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_data);
            break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                   /*FIX ME!!! log and quit*/
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            }
            else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));
            break;
        }
    }
    dump_gtp_pdp_create_response(response);
    return pdp_do_create_response(pkb);
}


void dump_gtp_pdp_update_request(struct pdp_update_req *req)
{
    GTP_DEBUG("pdp_update_request imsi:%llu "
              "teid_data:0x%08x teid_cp:0x%08x "
              "nsapi:%u  rat_type:%u "
              "sgsn_addr_cp:%d.%d.%d.%d sgsn_addr_data:%d.%d.%d.%d\n"
              "sgsn_addr_cp_alt:%d.%d.%d.%d sgsn_addr_data_alt:%d.%d.%d.%d\n",
              req->imsi, req->teid_data, req->teid_cp,
              req->nsapi, req->rat_type,
              _ADDR(req->gsn_addr_cp.addr4), _ADDR(req->gsn_addr_data.addr4),
              _ADDR(req->gsn_addr_cp_alt.addr4), _ADDR(req->gsn_addr_data_alt.addr4));
}

int gtp_do_update_pdp_request(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct pdp_update_req *request = (struct pdp_update_req *)gtpinfo->data;

    memset(request, 0, sizeof(struct pdp_update_req));
    end = tv + gtpinfo->payload_len;

    while (tv < end) {
        optcode = *tv;

        switch(optcode) {

        case GTP_EXT_IMSI:
            tv += decode_gtp_imsi(tv, &request->imsi);
            break;

        case GTP_EXT_TEID:
            tv +=  decode_gtp_teid_data1(tv, &request->teid_data);
            break;

        case GTP_EXT_TEID_CP:
            tv +=  decode_gtp_teid_cp(tv, &request->teid_cp);
            break;

        case GTP_EXT_NSAPI:
            tv += decode_gtp_nsapi(tv, &request->nsapi);
            break;

        case GTP_EXT_RAT_TYPE:
            tv += decode_gtp_rat_type(tv, &request->rat_type);
            break;

        case GTP_EXT_RAI:
            tv += decode_gtp_rai(tv, &request->rai);
            break;

        case GTP_EXT_GSN_ADDR:
            
            if (request->gsn_addr_cp.len == 0) 
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_cp);
            else if (request->gsn_addr_data.len == 0)
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_data);
            else if (request->gsn_addr_cp_alt.len == 0)
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_cp_alt);
            else
                tv += decode_gtp_gsn_addr(tv, &request->gsn_addr_data_alt);
                
            break;

		/*
		 *	guotie 2014-03-10
		 */
		case GTP_EXT_USR_LOC_INF:
			tv += decode_gtp_usr_loc_inf(tv, &request->location);
			break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            }
            else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));
            break;
        }
    }
    
    dump_gtp_pdp_update_request(request);
    return pdp_do_update_request(pkb);
}



void dump_gtp_pdp_update_response(struct pdp_update_resp *rsp)
{
    GTP_DEBUG("pdp_update_response cause:%u "
              "teid_data:0x%08x teid_cp:0x%08x "
              "ggsn_addr_cp:%d.%d.%d.%d ggsn_addr_data:%d.%d.%d.%d\n"
              "ggsn_addr_cp_alt:%d.%d.%d.%d ggsn_addr_data_alt:%d.%d.%d.%d\n",
              rsp->cause, rsp->teid_data, rsp->teid_cp,
              _ADDR(rsp->gsn_addr_cp.addr4), _ADDR(rsp->gsn_addr_data.addr4),
              _ADDR(rsp->gsn_addr_cp_alt.addr4), _ADDR(rsp->gsn_addr_data_alt.addr4));
}


int gtp_do_update_pdp_response(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct pdp_update_resp *response = (struct pdp_update_resp *)gtpinfo->data;

    memset(response, 0, sizeof(struct pdp_update_resp));
    end = tv + gtpinfo->payload_len;

    while (tv < end) {
        optcode = *tv;

        switch(optcode) {

        case GTP_EXT_CAUSE:
            tv += decode_gtp_cause(tv, &response->cause);
            break;

        case GTP_EXT_TEID:
            tv += decode_gtp_teid_data1(tv, &response->teid_data);
            break;

        case GTP_EXT_TEID_CP:
            tv += decode_gtp_teid_cp(tv, &response->teid_cp);
            break;
            
        case GTP_EXT_GSN_ADDR:

            if (response->gsn_addr_cp.len == 0) 
                 tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_cp);
            else if (response->gsn_addr_data.len == 0)
                 tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_data);
            else if (response->gsn_addr_cp_alt.len == 0)
                 tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_cp_alt);
            else
                 tv += decode_gtp_gsn_addr(tv, &response->gsn_addr_data_alt);
                     
            break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            }
            else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));
            break;
        }
    }
    dump_gtp_pdp_update_response(response);
    return pdp_do_update_response(pkb);
}


void dump_gtp_delete_pdp_request(struct pdp_delete_req *req)
{
    GTP_DEBUG("pdp_delete_request tear_id:%u nsapi:%u\n",
               req->tear_id, req->nsapi);
}

int gtp_do_delete_pdp_request(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct  gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct  pdp_delete_req *request = (struct pdp_delete_req *)gtpinfo->data;

    memset(request, 0, sizeof(struct pdp_delete_req));

    end = tv + gtpinfo->payload_len;
    while (tv < end) {
        optcode = *tv;

        switch(optcode) {
        case  GTP_EXT_TEAR_IND:
            tv += decode_gtp_tear_id(tv, &request->tear_id);
            break;
            
        case GTP_EXT_NSAPI:
            tv += decode_gtp_nsapi(tv, &request->nsapi);
            break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                   /*FIX ME!!! log and quit*/
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            }
            else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));
            break;
        }
    }
    dump_gtp_delete_pdp_request(request);
    pdp_do_delete_request(pkb);

    return 0;
}


void dump_gtp_delete_pdp_response(struct pdp_delete_resp *rsp)
{
    GTP_DEBUG("pdp_delete_response cause:%u \n", rsp->cause);
}

int  gtp_do_delete_pdp_response(struct pk_buff *pkb)
{
    u8 *tv = pkb->data, *end;
    u8 optcode;
    struct  gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;
    struct  pdp_delete_resp  *response = (struct pdp_delete_resp *)gtpinfo->data;

    memset(response, 0, sizeof(struct pdp_delete_resp));

    end = tv + gtpinfo->payload_len;
    while (tv < end) {
        optcode = *tv;
        switch(optcode) {
        case GTP_EXT_CAUSE:
            tv += decode_gtp_cause(tv, &response->cause);
            break;

        default:
            if (optcode <= 127) {
                if (0 == options[optcode].len) {
                   /*FIX ME!!! log and quit*/
                    record_buggy_packet(GTP_ERR, pkb);
                    return  -1;
                }
                tv += options[optcode].len;
            } else
                tv += (3 + (u16)ntohs(*(u16 *)(tv + 1)));
            break;
        }
    }
    dump_gtp_delete_pdp_response(response);
    pdp_do_delete_response(pkb);
    return 0;
}


int decode_gtp_header(struct pk_buff *pkb,  struct gtp_info *gtpinfo)
{
   struct gtphdr *gtphdr = pkb->gtphdr;
   u8 *buf = (u8 *)gtphdr;
   u8  version, proto_type;

//   AP_ASSERT(ntohs(gtphdr->len) <= pkb->len);

 //  if (ntohs(gtphdr->len) > pkb->len){
      // record_buggy_packet(GTP_ERR, pkb);
     //  return -1;
   //}
   
   /*gtp version check*/
   version = gtphdr->flag >> 5;
   proto_type = (gtphdr->flag & 0x10) >> 4;
   if (version != 1 || proto_type == 0)
      return -EGTP_VER_NOT_SUPPORT;

   /*decode gtp header*/
   gtpinfo->gsn_addr_dst = pkb->iphdr->daddr;
   gtpinfo->gsn_addr_src = pkb->iphdr->saddr;
   gtpinfo->msg = gtphdr->msg;
   gtpinfo->payload_len = (u16)ntohs(gtphdr->len);
   
   buf += sizeof(struct gtphdr);
   gtpinfo->teid = ntohl(*(u32 *)(buf));
   buf += 4;
   
   if (gtphdr->flag & 0x07) {
       if (gtphdr->flag & 0x02){
           gtpinfo->seq = ntohs(*(u16 *)buf);
           gtpinfo->flag |= 0x02;
       }
       buf += 2;
       gtpinfo->payload_len -= 2;
   }
   
   if (gtphdr->flag & 0x07) {
       if (gtphdr->flag & 0x01) {
           gtpinfo->pdu_no = *(buf);
           gtpinfo->flag |= 0x01; 
       }
       buf += 1;
       gtpinfo->payload_len -= 1;
   }
   
   if (gtphdr->flag & 0x07) {
      if (gtphdr->flag & 0x04) {
          gtpinfo->exh_type = *(buf);
          gtpinfo->flag |= 0x04;
      }
      buf += 1;
      gtpinfo->payload_len -= 1;
   }

   /* FIX ME!! to decode extend header*/
   if (gtpinfo->exh_type > 0)
      return  -EGTP_EXT_HEADER;

   return  buf - (u8 *)gtphdr;
}


int decode_gtp_u(struct pk_buff *pkb)
{
    int hdr_size = 0;
    int rx = 1;
    pkb->gtphdr = (struct gtphdr *)pkb->data;
    struct gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;

    memset(gtpinfo, 0, sizeof(*gtpinfo));
    hdr_size = decode_gtp_header(pkb, gtpinfo);
    if (hdr_size < 0)
        return (hdr_size); 

    pkb_pull(pkb, hdr_size);

    /*find pdp_context*/
    pkb->context = get_pdp_by_su(gtpinfo->gsn_addr_dst, gtpinfo->teid);
    if (!pkb->context) {
        rx = 0;
        pkb->context = get_pdp_by_gu(gtpinfo->gsn_addr_dst, gtpinfo->teid);    
    }
    
    if (!pkb->context)
        return  -EPKT_NO_PDPCNTXT;

    pthread_spin_lock(&pkb->context->lock_u);
    pkb->context->last_pkt = TIMESTAMP;
    if (rx) {
        pkb->context->pkts_rx++;
        pkb->context->bytes_rx += pkb->len;
    } else {
        pkb->context->pkts_tx++;
        pkb->context->bytes_tx+=pkb->len;
    }

    decode_user_pdu(pkb, rx);
    pthread_spin_unlock(&pkb->context->lock_u);

    put_pdp(pkb->context);
    return 0;
}

int decode_gtp_c(struct pk_buff *pkb)
{
    int hdr_size = 0;
    pkb->gtphdr = (struct gtphdr *)pkb->data;
    struct gtp_info *gtpinfo = (struct gtp_info *)pkb->cb;

	if(gtpc_cap_enable) {
		raw_pcap_dump_exclusive(gtpc_pcap, pkb->head, pkb->total);
	}

    memset(gtpinfo, 0, sizeof(*gtpinfo));
    hdr_size = decode_gtp_header(pkb, gtpinfo);
    if (hdr_size < 0)
        return (hdr_size); 
    
    pkb_pull(pkb, hdr_size);
    
    switch(gtpinfo->msg){

    case GTP_MSG_CREATE_PDP_REQ:
       return gtp_do_create_pdp_request(pkb);
       break;

    case GTP_MSG_CREATE_PDP_RESP:
        return gtp_do_create_pdp_response(pkb);
        break;

    case GTP_MSG_UPDATE_PDP_REQ:
        return gtp_do_update_pdp_request(pkb);
        break;

    case GTP_MSG_UPDATE_PDP_RESP:
        return gtp_do_update_pdp_response(pkb);
        break;

    case GTP_MSG_DELETE_PDP_REQ:
        return gtp_do_delete_pdp_request(pkb);
        break;

    case GTP_MSG_DELETE_PDP_RESP:
        return gtp_do_delete_pdp_response(pkb);
        break;

    default:
        return 0;
        break;
    }
    return 0;
}

/* ----------------------------------------------------------------------- */
/*console commands*/

#include "console.h"

int gtpc_port_handler(struct console_env *env, int argc, char **argv)
{
    gtpc_port = (u16)atoi(argv[1]);
    if (gtpc_port == 0) {
        console_printf(env, "error configure for gtpc port, using default\n");
        gtpc_port = GTPv1C_PORT;
    }
    return 0;
}

int gtpu_port_handler(struct console_env *env, int argc, char **argv)
{
    gtpu_port = (u16)atoi(argv[1]);
    if (gtpu_port == 0) {
        console_printf(env, "error configure for gtpu port, using default\n");
        gtpu_port = GTPv1U_PORT;
    }
    return 0;
}

int gtpc_cap_open() {
	char fname[128] = "";

	snprintf(fname, sizeof(fname) - 1, "/tmp/capture/gtpc-%s.pcap", datetime);
	gtpc_pcap = raw_pcap_open(fname);

	if(gtpc_pcap == NULL)
		return 1;

	return 0;
}

void gtpc_cap_close() {
	raw_pcap_close(gtpc_pcap);
	gtpc_pcap = NULL;
}


int gtpc_cap_handler(struct console_env *env, int argc, char **argv) {
	int ret = 0;
	
	if((strncasecmp(argv[1], "enable", 6) == 0) || (*argv[1] == '1')) {
		if(gtpc_cap_enable == 0) {
			ret = gtpc_cap_open();
			if(ret == 0)
				gtpc_cap_enable = 1;
			else {
				console_printf(env, "gtpc_cap open failed: retval = %d\n", ret);
			}
		}
	}
	else if ((strncasecmp(argv[1], "disable", 7) == 0) || (*argv[1] == '0')) {
		if(gtpc_cap_enable == 1) {
			gtpc_cap_enable = 0;
			gtpc_cap_close();
		}
	}
	else
		console_printf(env, "gtpc_cap: invalide param: %s, should be enable/disable/1/0.\n", argv[1]);

	return 0;
}

DECLARE_CMD(gtpc_port, 2, 2,0, "set udp port of gtp control signal",
            "set udp port of gtp control signal --FMT: gtpc_port port")

DECLARE_CMD(gtpu_port, 2, 2,0, "set udp port of gtp data signal",
            "set udp port of gtp data signal --FMT: gtpu_port port")

DECLARE_CMD(gtpc_cap, 2, 2,0, "enable/disable gtp_c capture",
            "enable/disable gtp_c capture --FMT: gtpc_cap enable/disable")


#endif  /* endof defined(CONFIG_NETWORK_WCDMA) */



/*test codes*/
#if 0


/*test codes here*/
char pcapfile[256] = "eth1.cap";

int main(int argc, char ** argv)
{
    char errbuf[PCAP_ERRBUF_SIZE] = {0};
    pcap_t *cap_file = NULL;
    const unsigned char *pkt_data = NULL;
    struct pcap_pkthdr *pkt_hdr = NULL;
    int status;
//    u32 pkt_id = 1;
    struct pk_buff  zb = {0};
    
    gtp_opt_init();
    init_pdp_context_storage();
    
 # if 1
    cap_file = pcap_open_offline(pcapfile, errbuf);

    if (!cap_file) {
        printf("%s\n", errbuf);
        return -1;
    }
    status = pcap_next_ex(cap_file, &pkt_hdr, &pkt_data);

    while(status == 1) {

        #if 0
        printf("pkt_id %u ", pkt_id++);
        printf("cap_time:%u ",(unsigned int)pkt_hdr->ts.tv_sec);
        printf("pkt_len:%u ", pkt_hdr->len);
        printf("cap_len:%u\n", pkt_hdr->caplen);
        #endif

        memset(&zb, 0, sizeof(struct pk_buff));

        zb.head = zb.data = (u8 *)pkt_data;
        zb.total = zb.len = pkt_hdr->caplen;

        decode_packet(&zb);
        status = pcap_next_ex(cap_file, &pkt_hdr, &pkt_data);
    }
#endif
     clean_pdp_context_storage();
     gtp_opt_clean();

    return 0;
}

#endif

